#ifndef __ASTAR_WORLD_VECTOR3_H__
#define __ASTAR_WORLD_VECTOR3_H__

#include <map>
#include <memory>
#include "PathfindingResult.hpp"
#include "Engine/BoundingBox.hpp"

namespace PocketSurvival
{
    class SubsystemTerrain;
    class Block;

    struct Request
    {
    public:
        Vector3 Start;
        Vector3 End;
        Vector3 BoxSize;
        float MinDistance;
        int32_t MaxPositionsToCheck;
        PathfindingResult *PathFinding;
    };

    class AStarWorldVector3
    {
    public:
        SubsystemTerrain *terrain;
        Request request;

    public:
        AStarWorldVector3();

        inline float cost(const Vector3 &p1, const Vector3 &p2);

        inline float heuristic(const Vector3 &p1, const Vector3 &p2);

        inline bool IsGoal(const Vector3 &p);

        void getNeighbors(const Vector3 &p, std::vector<Vector3> &neighbors);

        void addNeighbor(std::vector<Vector3> &neighbors, const Vector3 &p, int32_t dx, int32_t dz);

        float getBlockWalkingHeight(Block *block, int32_t value);

        bool isBlocked(const BoundingBox &box);
    };

    inline float AStarWorldVector3::cost(const Vector3 &p1, const Vector3 &p2)
    {
        return 0.999f - 0.1f * Vector3::Dot(Vector3::Normalize(p2 - p1), Vector3::Normalize(request.End - p1));
    }

    inline float AStarWorldVector3::heuristic(const Vector3 &p1, const Vector3 &p2)
    {
        float num = MathUtils::Abs(p1.posX - p2.posX);
        float num2 = MathUtils::Abs(p1.posZ - p2.posZ);
        if (num > num2)
        {
            return 1.41f * num2 + 1.0f * (num - num2);
        }
        return 1.41f * num + 1.0f * (num2 - num);
    }

    inline bool AStarWorldVector3::IsGoal(const Vector3 &p)
    {
        return Vector3::DistanceSquared(p, request.End) <= (request.MinDistance * request.MinDistance);
    }
}

#endif
